#include "contact.h"
#include <cassert>
using namespace TorPhysix;


// Construct an arbitrary orthonormal basis, this is stored as a 3x3 matrix
// the x-direction is generated from the contact normal and the y and z directions
// are set so they are at right angles to it
inline
void Contact::calculateContactBasis()
{
	Vec3f contactTangent[2];

	// Check whether the Z-axis is nearer to the X or Y axis
	if (real_abs(contactNormal.x() ) > real_abs(contactNormal.y() ))
	{
		// Scaling factor to ensure the results are normalized
		const real s = (real)1.0f/real_sqrt( contactNormal.z()*contactNormal.z() + contactNormal.x()*contactNormal.x() );

		// The new X-axis is at right angles to the world Y-axis
		contactTangent[0].x() = contactNormal.z()*s;
		contactTangent[0].y() = 0;
		contactTangent[0].z() = -contactNormal.x()*s;

		// The new Y-axis is at right angles to the new X- and Z- axes
		contactTangent[1].x() =  contactNormal.y() * contactTangent[0].x();
		contactTangent[1].y() =  contactNormal.z() * contactTangent[0].x() - contactNormal.x() * contactTangent[0].z();
		contactTangent[1].z() = -contactNormal.y() * contactTangent[0].x();
	}
	else
	{
		// Scaling factor to ensure the results are normalized
		const real s = (real)1.0/real_sqrt(contactNormal.z()*contactNormal.z() + contactNormal.y()*contactNormal.y() );

		// The new X-axis is at right angles to the world X-axis
		contactTangent[0].x() = 0;
		contactTangent[0].y() = -contactNormal.z()*s;
		contactTangent[0].z() = contactNormal.y()*s;

		// The new Y-axis is at right angles to the new X- and Z- axes
		contactTangent[1].x() = contactNormal.y()*contactTangent[0].z() - contactNormal.z()*contactTangent[0].y();
		contactTangent[1].y() = -contactNormal.x()*contactTangent[0].z();
		contactTangent[1].z() = contactNormal.x()*contactTangent[0].y();
	}

	// Make a matrix from the three vectors.
	contactToWorld = Mat3f( contactNormal, contactTangent[0], contactTangent[1] );
	//contactToWorld[0] = contactNormal.x();
	//contactToWorld[1] = contactTangent[0].x();
	//contactToWorld[2] = contactTangent[1].x();
	//contactToWorld[3] = contactNormal.y();
	//contactToWorld[4] = contactTangent[0].y();
	//contactToWorld[5] = contactTangent[1].y();
	//contactToWorld[6] = contactNormal.z();
	//contactToWorld[7] = contactTangent[0].z();
	//contactToWorld[8] = contactTangent[1].z();
}

// sets contactVelocity for use in the adjustPosition step
// and calls calcDesiredDeltaVelocity, so desiredDeltaVelocity is ready for adjustVelocity step
void Contact::calculateInternals(real duration)
{
	// Check if the first object is NULL, and swap if it is.
	if (!body[0]) swapBodies();
	assert(body[0]);

	// Calculate an set of axis at the contact point.
	calculateContactBasis();

	// Store the relative position of the contact relative to each body
	relativeContactPosition[0] = contactPoint - body[0]->getPosition();
	if (body[1]) {
		relativeContactPosition[1] = contactPoint - body[1]->getPosition();
	}

	// Find the relative velocity of the bodies at the contact point.
	// This is used when correcting position
	contactVelocity = calculateLocalVelocity(0, duration);

 	if (body[1]) {
		contactVelocity -= calculateLocalVelocity(1, duration); // We want velocity to be relative to contacts.
	}

	// Calculate the desired change in velocity for resolution
	calculateDesiredDeltaVelocity(duration);
}

void Contact::setBodyData(RigidBody* one, RigidBody *two, real friction, real restitution)
{
	Contact::body[0] = one->isStatic?NULL:one;
    Contact::body[1] = two->isStatic?NULL:two;
    Contact::friction = friction;
    Contact::restitution = restitution;
}


/*
 * Swaps the bodies in the current contact, so body 0 is at body 1 and
 * vice versa. This also changes the direction of the contact normal,
 * but doesn't update any calculated internal data. If you are calling
 * this method manually, then call calculateInternals afterwards to
 * make sure the internal data is up to date.
 */
void Contact::swapBodies()
{
    contactNormal *= -1;

    RigidBody *temp = body[0];
    body[0] = body[1];
    body[1] = temp;
}

// Gives us the velocity of one body relative to the other at the contact point in contact coordinates,
// that means the velocity is in x-axis.
Vec3f Contact::calculateLocalVelocity(unsigned bodyIndex, real duration)
{
    RigidBody *thisBody = body[bodyIndex];

    // Work out the velocity of the contact point.
	Vec3f contactVelocity = thisBody->getRotation().cross( relativeContactPosition[bodyIndex] );
    contactVelocity += thisBody->getVelocity();

    // Turn the velocity into contact-coordinates.
	contactVelocity = transformTranspose(contactToWorld, contactVelocity);

    // Calculate the amount of velocity that is due to forces without
    // reactions.
    Vec3f accVelocity = thisBody->getLastFrameAcceleration() * duration;

    // Calculate the velocity in contact-coordinates.
    accVelocity = transformTranspose(contactToWorld, accVelocity);

    // We ignore any component of acceleration in the contact normal
    // direction, we are only interested in planar acceleration
    accVelocity.x() = 0;

    // Add the planar velocities - if there's enough friction they will
    // be removed during velocity resolution
    contactVelocity += accVelocity;

    // And return it
    return contactVelocity;
}


// part of calculate internals, gives us the delta adjustVelocity step
void Contact::calculateDesiredDeltaVelocity(real duration)
{
    const static real velocityLimit = (real)0.25f;

    // Calculate the acceleration induced velocity accumulated this frame
    real velocityFromAcc = 0;

    velocityFromAcc = body[0]->getLastFrameAcceleration().dot(contactNormal) * duration;

    if( body[1] )
    {
        velocityFromAcc -= body[1]->getLastFrameAcceleration().dot(contactNormal) * duration;
    }

    // If the velocity is very slow, limit the restitution
    real thisRestitution = restitution;
    if( real_abs(contactVelocity.x()) < velocityLimit )
    {
        thisRestitution = (real)0.0f;
    }

    // Combine the bounce velocity with the removed acceleration velocity.
	desiredDeltaVelocity = -contactVelocity.x() -thisRestitution * (contactVelocity.x() - velocityFromAcc);
}


// is called by Resolver
void Contact::applyVelocityChange(Vec3f velocityChange[2], Vec3f rotationChange[2])
{
    // Get hold of the inverse mass and inverse inertia tensor, both in
    // world coordinates.
    Mat3f inverseInertiaTensor[2];
    body[0]->getInverseInertiaTensorWorld(&inverseInertiaTensor[0]);
    if (body[1])
        body[1]->getInverseInertiaTensorWorld(&inverseInertiaTensor[1]);

    // We will calculate the impulse for each contact axis
    Vec3f impulseContact;

    if (friction == (real)0.0)
    {
        // Use the short format for frictionless contacts
		// if the collision was head-on (the collision normal is in the same direction as velocity)
		// then there should only be an impulse in x-axis
        impulseContact = calculateFrictionlessImpulse(inverseInertiaTensor); 
    }
    else
    {
        // Otherwise we may have impulses that aren't in the direction of the
        // contact, so we need the more complex version.
        impulseContact = calculateFrictionImpulse(inverseInertiaTensor);
    }

    // Convert impulse to world coordinates
    Vec3f impulse = contactToWorld * impulseContact;

    // Split in the impulse into linear and rotational components
    Vec3f impulsiveTorque = relativeContactPosition[0].cross(impulse);
    rotationChange[0] = inverseInertiaTensor[0] * impulsiveTorque;
    velocityChange[0].is(0,0,0); // clear
    velocityChange[0] += impulse * body[0]->getInverseMass(); // add the scaled impulse

    // Apply the changes
    body[0]->addVelocity(velocityChange[0]);
    body[0]->addRotation(rotationChange[0]);

    if (body[1])
    {
        // Work out body one's linear and angular changes
        Vec3f impulsiveTorque = impulse.cross( relativeContactPosition[1] );
        rotationChange[1] = inverseInertiaTensor[1] * impulsiveTorque;
        velocityChange[1].is(0,0,0); // clear
        velocityChange[1] += impulse * -body[1]->getInverseMass(); // add scaled impulse, reversed direction

        // And apply them.
        body[1]->addVelocity(velocityChange[1]);
        body[1]->addRotation(rotationChange[1]);
    }
}

inline
Vec3f Contact::calculateFrictionlessImpulse(Mat3f * inverseInertiaTensor)
{
    Vec3f impulseContact;

    // Build a vector that shows the change in velocity in
    // world space for a unit impulse in the direction of the contact
    // normal.
    Vec3f deltaVelWorld = relativeContactPosition[0].cross(contactNormal);
    deltaVelWorld = inverseInertiaTensor[0] * deltaVelWorld;
    deltaVelWorld = deltaVelWorld.cross(relativeContactPosition[0]);

    // Work out the change in velocity in contact coordinates.
    real deltaVelocity = deltaVelWorld.dot(contactNormal);

    // Add the linear component of velocity change
    deltaVelocity += body[0]->getInverseMass();

    // Check if we need to the second body's data
    if (body[1])
    {
        // Go through the same transformation sequence again
        Vec3f deltaVelWorld = relativeContactPosition[1].cross(contactNormal);
        deltaVelWorld = inverseInertiaTensor[1] * deltaVelWorld;
        deltaVelWorld = deltaVelWorld.cross(relativeContactPosition[1]);

        // Add the change in velocity due to rotation
        deltaVelocity += deltaVelWorld.dot(contactNormal);

        // Add the change in velocity due to linear motion
        deltaVelocity += body[1]->getInverseMass();
    }

    // Calculate the required size of the impulse
    impulseContact.x() = desiredDeltaVelocity / deltaVelocity;
    impulseContact.y() = 0;
    impulseContact.z() = 0;
    return impulseContact;
}

inline
Vec3f Contact::calculateFrictionImpulse(Mat3f * inverseInertiaTensor)
{
    Vec3f impulseContact;
    real inverseMass = body[0]->getInverseMass();

    // The equivalent of a cross product in matrices is multiplication
    // by a skew symmetric matrix - we build the matrix for converting
    // between linear and angular quantities.
    Mat3f impulseToTorque;
    setSkewSymmetric(impulseToTorque, relativeContactPosition[0]);

    // Build the matrix to convert contact impulse to change in velocity
    // in world coordinates.
    Mat3f deltaVelWorld = impulseToTorque;
    deltaVelWorld *= inverseInertiaTensor[0];
    deltaVelWorld *= impulseToTorque;
    deltaVelWorld *= -1;

    // Check if we need to add body two's data
    if (body[1])
    {
        // Set the cross product matrix
        setSkewSymmetric(impulseToTorque, relativeContactPosition[1]);

        // Calculate the velocity change matrix
        Mat3f deltaVelWorld2 = impulseToTorque;
        deltaVelWorld2 *= inverseInertiaTensor[1];
        deltaVelWorld2 *= impulseToTorque;
        deltaVelWorld2 *= -1;

        // Add to the total delta velocity.
        deltaVelWorld += deltaVelWorld2;

        // Add to the inverse mass
        inverseMass += body[1]->getInverseMass();
    }

    // Do a change of basis to convert into contact coordinates.
    Mat3f deltaVelocity = transpose(contactToWorld);
    deltaVelocity *= deltaVelWorld;
    deltaVelocity *= contactToWorld;

    // Add in the linear velocity change
    deltaVelocity[0] += inverseMass;
    deltaVelocity[4] += inverseMass;
    deltaVelocity[8] += inverseMass;

    // Invert to get the impulse needed per unit velocity
    Mat3f impulseMatrix = inverse(deltaVelocity);

    // Find the target velocities to kill
    Vec3f velKill( desiredDeltaVelocity, -contactVelocity.y(), -contactVelocity.z() );

    // Find the impulse to kill target velocities
    impulseContact = impulseMatrix * velKill;

    // Check for exceeding friction
    real planarImpulse = real_sqrt( impulseContact.y()*impulseContact.y() + impulseContact.z()*impulseContact.z() );
    if (planarImpulse > impulseContact.x() * friction)
    {
        // We need to use dynamic friction
        impulseContact.y() /= planarImpulse;
        impulseContact.z() /= planarImpulse;

        impulseContact.x() = deltaVelocity[0] + deltaVelocity[1]*friction*impulseContact.y() + deltaVelocity[2]*friction*impulseContact.z();
        impulseContact.x() = desiredDeltaVelocity / impulseContact.x();
        impulseContact.y() *= friction * impulseContact.x();
        impulseContact.z() *= friction * impulseContact.x();
    }
    return impulseContact;
}

/**
 * Performs an inertia weighted penetration resolution of this
 * contact alone.
 */
void Contact::applyPositionChange(Vec3f linearChange[2], Vec3f angularChange[2], real penetration)
{
    const real angularLimit = (real)0.2f;
    real angularMove[2];
    real linearMove[2];

    real totalInertia = 0;
    real linearInertia[2];
    real angularInertia[2];

    // We need to work out the inertia of each object in the direction
    // of the contact normal, due to angular inertia only.
    for (unsigned i = 0; i < 2; i++) if (body[i])
    {
        Mat3f inverseInertiaTensor;
        body[i]->getInverseInertiaTensorWorld(&inverseInertiaTensor);

        // Use the same procedure as for calculating frictionless
        // velocity change to work out the angular inertia.
        Vec3f angularInertiaWorld = relativeContactPosition[i].cross(contactNormal);
        angularInertiaWorld = inverseInertiaTensor * angularInertiaWorld;
        angularInertiaWorld = angularInertiaWorld.cross(relativeContactPosition[i]);
        angularInertia[i] = angularInertiaWorld.dot(contactNormal);

        // The linear component is simply the inverse mass
        linearInertia[i] = body[i]->getInverseMass();

        // Keep track of the total inertia from all components
        totalInertia += linearInertia[i] + angularInertia[i];

        // We break the loop here so that the totalInertia value is
        // completely calculated (by both iterations) before
        // continuing.
    }

    // Loop through again calculating and applying the changes
    for (unsigned i = 0; i < 2; i++) if (body[i])
    {
        // The linear and angular movements required are in proportion to
        // the two inverse inertias.
        real sign = (i == 0)?1.0f:-1.0f;
        angularMove[i] = sign * penetration * (angularInertia[i] / totalInertia);
        linearMove[i] = sign * penetration * (linearInertia[i] / totalInertia);

        // To avoid angular projections that are too great (when mass is large
        // but inertia tensor is small) limit the angular move.
        Vec3f projection = relativeContactPosition[i];
        projection += contactNormal * -relativeContactPosition[i].dot(contactNormal);

        // Use the small angle approximation for the sine of the angle (i.e.
        // the magnitude would be sine(angularLimit) * projection.magnitude
        // but we approximate sine(angularLimit) to angularLimit).
        real maxMagnitude = angularLimit * length(projection);

        if (angularMove[i] < -maxMagnitude)
        {
            real totalMove = angularMove[i] + linearMove[i];
            angularMove[i] = -maxMagnitude;
            linearMove[i] = totalMove - angularMove[i];
        }
        else if (angularMove[i] > maxMagnitude)
        {
            real totalMove = angularMove[i] + linearMove[i];
            angularMove[i] = maxMagnitude;
            linearMove[i] = totalMove - angularMove[i];
        }

        // We have the linear amount of movement required by turning
        // the rigid body (in angularMove[i]). We now need to
        // calculate the desired rotation to achieve that.
        if (angularMove[i] == 0)
        {
            // Easy case - no angular movement means no rotation.
            angularChange[i].is(0,0,0); //clear
        }
        else
        {
            // Work out the direction we'd like to rotate in.
            Vec3f targetAngularDirection = relativeContactPosition[i].cross(contactNormal);

            Mat3f inverseInertiaTensor;
            body[i]->getInverseInertiaTensorWorld(&inverseInertiaTensor);

            // Work out the direction we'd need to rotate to achieve that
            angularChange[i] = inverseInertiaTensor * targetAngularDirection * (angularMove[i] / angularInertia[i]);
        }

        // Velocity change is easier - it is just the linear movement
        // along the contact normal.
        linearChange[i] = contactNormal * linearMove[i];

        // Now we can start to apply the values we've calculated.
        // Apply the linear movement
        Vec3f pos = body[i]->getPosition();
        pos += contactNormal * linearMove[i]; // scaled
        body[i]->setPosition(pos);

        // And the change in orientation
  //      Quatf q;
  //      body[i]->getOrientation(&q);
  //      Vec3f scaledAngle = angularChange[i] * (real)1.0; // 1.0 means no scale.
  //      q *= Quatf(0.0f, scaledAngle );
  // Equivalent code:

		Quatf q;
		body[i]->getOrientation(&q);
		addScaledVector(q, angularChange[i], 1.0f);
        body[i]->setOrientation(q); 
    }
}